#include "stdafx.h"

#include "LocatorPoseMex.h"

void runLocatorInitialPose(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
	if (nrhs != 3) mexErrMsgTxt("LocatorInitialPose needs 2 arguments.");
	if (mxGetN(prhs[1]) != 3) mexErrMsgTxt("xmarker must be a Nx3 matrix.");
	if (mxGetN(prhs[2]) != 3) mexErrMsgTxt("xmodel must be an Nx3 matrix.");
	
	if ( (mxGetM(prhs[1]) != mxGetM(prhs[2])) )
		mexErrMsgTxt("All input variables must habe the same number of rows.");
		
	arma::mat xmarker = convertMatlabToArma(prhs[1]);		// x_marker
	arma::mat xmodel = convertMatlabToArma(prhs[2]);		// x_model

	arma::colvec xlocator;
		
	OTPF::Locator::locatorInitialPose(xmarker, xmodel, xlocator);
	
	plhs[0] = convertArmaToMatlab(xlocator);			// x_locator
};